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Path  Planning  in  Three  Dimensional  Environment  using  Feedback 

Linearization 

Shreecharan  Kanchanavally,  Raul  Ordonez,  Corey  J.  Schumacher 


Abstract —  This  paper  presents  a  control  scheme  via  feedback 
linearization  for  three  dimensional  cooperative  path  planning 
of  a  class  of  interconnected  systems  in  general  and  UAVs  in 
particular.  It  is  shown  that  the  feedback  linearization  technique 
along  with  a  distance  varied  repulsive  profile  allows  UAVs  to 
converge  to  the  invariant  set  of  a  known  target  location  without 
colliding  with  other  vehicles.  Lyapunov  stability  analysis  shows 
the  conditions  under  which  such  systems  are  stable.  Also  a  task 
assignment  algorithm,  which  is  a  function  of  distance  between 
the  UAVs  and  the  target,  is  proposed  for  dealing  with  multi 
UAV  and  multi  target  scenario. 

I.  Problem  Statement 

The  concept  of  autonomous  cooperative  vehicle  has  re¬ 
cently  found  applications  in  many  fields,  from  managing  a 
team  of  mobile  agents  for  fire  fighting  or  military  mission, 
to  coordinating  a  group  of  rovers  to  explore  an  unknown 
environment  or  perform  search  and  rescue  scenarios.  A  key 
problem  in  the  field  of  mobile  robotics  is  navigation  through 
an  unknown  environment  of  n  dimensions.  Navigation  or 
path  planning  means  to  determine  how  to  move  an  object 
form  its  original  location  and  orientation  to  a  goal  configu¬ 
ration  while  avoiding  collision  with  obstacles.  A  particularly 
challenging  instance  of  this  problem  is  flight  of  fixed  wing 
Uninhabited  Air  Vehicles  (UAV)  through  a  three  dimensional 
(3D)  environment.  In  particular  the  problem  becomes  even 
more  challenging  for  cooperative  path  planning  of  group  of 
UAVs  flying  in  a  3D  environment. 

In  the  past  feedback  linearization  technique  was  used  for 
formation  control  of  group  of  nonholonomic  vehicles  in  two 
dimensional  environment  [1].  The  concept  of  path  planning 
to  handle  formation  control  of  biological  creatures  using 
attraction/repulsion  function  was  used  in  [2] -[4].  A  receding 
horizon  control  [5]  can  be  used  to  design  trajectories  for 
an  aerial  vehicle  fling  through  a  three  dimensional  terrain 
with  obstacle  and  no-  fly  zones.  The  paper  [6]  proposes 
simple  reactive  feedback  laws  that  enable  obstacle  avoidance 
for  a  hover-capable  unmanned  aerial  vehicle  (UAV)  using 
a  crude  low  resolution,  small  field  of  view  radar  sensor. 
A  two  dimensional  potential  field  [7]  is  used  to  show  that 
equilibrium  is  a  global  minimum  of  the  sum  of  all  artificial 
potential  functions  and  is  locally  asymptotically  stable  for 
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the  closed  loop  dynamics  via  a  Lyapunov  stability  analysis 
for  a  system  with  n  agents  and  m  virtual  leaders.  The 
work  in  [8]  uses  input  to  state  stability  a  general  class  of 
formation  keeping  control  schemes  with  a  new  dynamic 
window  approach  to  obstacle  avoidance  in  order  to  guarantee 
safety  and  stability  of  formation  as  well  as  convergence 
of  the  goal  position.  A  decentralized  control  method  [9], 
along  with  cooperative  biological  swarming  models  with 
virtual  attractive  and  repulsive  potential  can  be  used  for  path 
planning  of  non  linear  second  order  dynamic  vehicles.  The 
paper  [10]  considers  the  problem  of  computing  shortest  ob¬ 
stacle  avoidance  paths  among  obstacles  in  three  dimensions. 
In  [11]  a  three  dimensional  probabilistic  approach  for  the 
path  planning  of  uninhabited  air  vehicles  is  presented.  The 
work  [12]  addresses  design  of  a  hierarchical  system  that 
uses  a  given  set  of  possibly  heterogeneous  UAVs  to  provide 
protection  to  a  moving  convoy  of  ground  vehicles.  The 
system  encompasses  task  generation  and  allocation,  flight 
path  generation  and  tracking,  and  synchronization  between 
cooperative  tasks.  The  use  of  artificial  potential  field  for 
mechanism  of  obstacle  avoidance  for  wheeled  rovers  used 
in  the  exploration  of  Mars  was  discussed  in  [13]. 

This  paper  discusses  the  problem  of  cooperative  control 
for  a  group  of  UAVs  having  motion  constraints  and  flying 
in  a  3D  environment  without  colliding  with  each  other  and 
reaching  their  dynamically  assigned  goal  positions.  In  the 
past,  vehicle  guidance  algorithms  that  avoid  obstacles  or 
other  vehicles  have  been  well  studied  assuming  that  the 
vehicle  remains  in  a  horizontal  plane  so  that  path  planning  is 
two  dimensions.  We  use  a  feedback  linearizing  controller  for 
path  planning,  varying  potential  field  for  collision  avoidance 
and  varying  references  for  task  assignments  of  the  UAVs. 

The  remainder  of  the  paper  is  organized  as  follows. 
Section  II  deals  with  the  stability  analysis  of  interconnected 
MIMO  systems  using  feedback  control  in  general.  Section 
III  holds  the  main  results  of  cooperative  path  planning  of 
UAVs  in  3D  environment  and  task  assignment  algorithm  with 
the  presence  of  virtual  target  for  dealing  with  multi  UAV 
and  multi  target  case.  Section  IV  presents  some  concluding 
remarks  along  with  some  future  research  directions. 


II.  MIMO  Feedback  Controls 

Consider  a  composite  system  composed  of  N  intercon¬ 
nected  heterogenous  square  MIMO  (Multi  Input  Multi  Out- 
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put)  systems  of  the  form 

x*  =f{xi)  +  g\(xl)u\  +  g\(xl)u\  +  •  •  •  +  g^x^u^ 

y\  =K{xl) 

yl2  =hl2(xl) 


yin  =hln(x2) 


(1) 


where  x1  G  lZn  represents  the  states  of  the  ith  subsystem, 
ul  G  7 Z171  represents  the  controllers  of  the  ith  subsystem 
and  yl  G  7Zm  represents  the  output  of  the  ith  subsystem. 
Assume  that  /\  gL-  and  In1-  are  smooth.  Define  rl-  to  be  the 
relative  degree  for  the  jth  output  of  the  ith  subsystem  such 
that  differentiating  y l-,  with  respect  to  xl,  times  will  result 


in  at  least  one  of  the  inputs  appearing  in  y 


K) 


,  i.e., 


yfr]) 

aUxi)  Plj(xi) 


(2) 


where  LfM  is  the  Lie  derivative  and  is  defined  as  Lfi  h  = 
§£:fi  and  L2^  h  =  Lji  (Lfi  h),  with  at  least  one  of 

Y>%  —  .  . 

Lg,.(Ljt  hlj)  ^  0.  Define  an  m  x  m  matrix  Bl(xl)  and 
a  m  x  1  matrix  A1  (xl)  for  each  subsystem  such  that 


"  ^11(2;) 

^12(2;)  •  • 

.  Pimix1)' 

P2l(x') 
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/ml^) 
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Then  equation  (2)  for  the  ith  subsystem  can  be  written  as 


1 

tO 

_ 1 

=A\xi)  + 

u\ 

u2 

i  (rm) 

[Vm  J 

-Um_ 

(4) 


rim  —  n 


Define  rl  =  [r\:rl2, . . . , rjj  ,  rl  =  r\  +  r\  +  • 

for  i  =  1, 2, . . . ,  TV  as  the  vector  relative  degree  of  the  ith 
subsystem.  We  assume  that  the  relative  degree  for  all  the 
subsystems  are  equal  in  the  sense  rl  =  r-7  with  r\  =  r3k  for 
k  =  1,  2, . . . ,  m.  However,  we  allow  the  actual  dynamics  of 
each  subsystem  (given  by  fl ,  g1-,  h1-)  to  be  different. 

We  assume  that  Bl(xl)  is  invertible.  Then  the  MIMO 
feedback  linear  control  is  given  by 


=Bi(xi)  [-A^x1)  jrV] 


(5) 


If  Bl(xl)  is  not  invertible,  feedback  linearization  is  still 
possible  using  dynamic  feedback  linearization  which  is  not 


discussed  in  this  paper.  Moreover  we  assume  that  each 
subsystem’s  zero  dynamics  are  input  to  state  stable. 

Define  a  smooth  reference  signal  Rref  as 

’nW 


Rref 


72  (f) 

7 m{t) 


(6) 


Also  define  the  error  between  the  jth  output  of  the  ith 
subsystem  and  reference  signal  7 j  e  Rref  as 

ej  =ki,i(Vj  ~  7 j)  +  k2,i{y)  -7 })  +  ■■■ 


,  2)  (r!-— 2)v  ,  i(r)- 1) 

+  K'-i M  -  7 j  )  +  {Vj  J  -  7 j  ) 

(7) 


so  that  differentiating  the  error  results  in 

e)  =h,i(Vj  -7j)  +  •••  +  kri_lti(yfrj  1}  -  7^  1}) 

+  (vp)-  i{A) 

=ki,i{y)  -ij)  +  ---  +  kri  hi{yfrj  1}  —  kj'i  1))-7j(l’3') 


+  y 


(r)) 


=  h,i(Vj  -7 i)4 - i(y: 


»  A)  n)_-vrj> 


+  alj(xl)  +  y ~^Pj,k(xl)uk 


k=l 

Define  the  error  vector  for  the  ith  subsystem  as  El  = 
[e\,  e|, . . . ,  elrn]T  and  for  the  composite  system  as  E  = 
[ElT ,  E2T , . . . ,  EnT]t .  We  define  the  error  dynamics  from 
equation  (7)  for  the  ith  subsystem  as 


(8) 


r  pi  1 

ei 

pi 

e2 

= 

"xi" 

xi 

+Ai(xi)  +  Bi(xi) 

1 

. .  ss  ss 

_ 1 

pi 

\_  mJ 

_A  m_ 

El  X% 

Substituting  the  control  equation  discussed  in  equation  (5), 
where  we  select  vl  =  —  x1  +  KlEl  with  Kl  a  mxm  matrix 
having  its  eigenvalues  in  the  left  half  plane,  we  get 

E{  =KiEi  (9) 

To  study  the  stability  of  the  error  dynamics  for  each  sub¬ 
system  let  us  define  the  Lyapunov  function,  with  a  posi¬ 
tive  definite  matrix  Pl ,  the  solution  of  Lyapunov  equation 

KiTpi  +  p>K>  =  -Q\  as 


Vi  =EiTPiEi  (10) 

Differentiating  V1  we  obtain 

V 1  =EiT  PiEi  +  EiTPiEi 
=EiT  [KiT  Pl  +  PiKi]Ei 
=  -  E^QtE1 


2 


We  notice  that  V1  is  negative  definite  thus  driving  El  to 
zero  exponentially.  To  study  the  stability  of  the  composite 
systems  we  define  the  composite  Lyapunov  function  as 

N 

V(E)  =J2vi(Ei)  (11) 

Z—  1 

For  any  positive  symmetric  matrix  S  and  vector  X  we  have 

A min(S)XTX  <  XTSX  <  A max(S)XTX 

where  A min(S)  and  A max(S)  are  the  minimum  and  maxi¬ 
mum  eigenvalues  of  S  respectively.  So  we  have 

N 

v  =  -J2EiTQiEi 

N 

<-^Amin(Qi)||Ei||2 

i= 1 

V  to  be  negative  definite  thereby  regulating  the  overall  error 
to  zero. 


A.  Interconnection  Between  the  Subsystems 

A  certain  class  of  interconnected  systems  does  not  allow 
some  or  all  the  states  of  the  subsystem  to  be  equal  at  a  time 
instance.  Examples  of  such  class  of  systems  include  groups 
of  UAVs  with  position  of  each  UAV  as  the  state.  In  order 
to  deal  with  such  type  of  systems  we  introduce  the  inter¬ 
connectivity  among  the  subsystems  via  a  repulsive  force  that 
surrounds  each  subsystem.  We  define  the  force  of  repulsion 
F*  that  surrounds  ith  subsystem  as 


N 

Flr  —  ^  Kj  exp 

j=l, j^i 


xl  —  xi\\2- 
2  rf  - 


(4 


yj  +  9)  (12) 


To  study  the  stability  of  the  error  dynamics  defined  by 
the  composite  systems  in  the  presence  of  interconnection, 
we  differentiate  the  composite  Lyapunov  function  defined  in 
equation  (11)  to  obtain 


V(E)  =  Y{~  EiJQiEi  +  2 EiTPlF^ 

i=l 
N 

<E(-WQ‘)M2 

1  N 

+  2||Pi||Amax(JPi)exp(--)  Y,  (ri  +  *)) 

N 

<E(- wq‘) 


Z—  1 


\El  II- 


2Amax(P») 

Amin(Q*)  .  "“P. 


N 

E  w +'£')]  i  i^ii) 


N 

It  is  clear  that  when  \\El\  \  —  2^max^^  (rJs  +  T^)  >  0, 


i=l,z#j 

V{E )  <  0.  Define  VL  as  the  invariant  set  for  the  ith  vehicle 

N 


such  that  ST  =  G  TZm  :  Ei  > 

Z=1 

Hence  it  is  clear  that  whenever  the  error  vector  is  outside 
fT,  it  is  driven  to  the  invariant  set 


III.  Application  to  UAV  case 
A.  UAV  Model 

We  consider  the  four  DOF  model  of  the  UAV  with  seven 
states  given  by 


where  Kj  represents  a  repulsive  constant,  ^  is  a  design 
constant  whose  significance  is  explained  in  Section  III-C  and 
rJs  represents  the  surrounding  or  spread  in  which  the  effect 
of  force  is  realized.  The  function  has  the  significance  that  as 
the  states  are  far  apart  from  each  other  the  force  of  repulsion 
is  less  as  compared  to  when  the  states  are  closer.  Also 
it  has  the  significance  that  there  exists  a  certain  minimum 
force  of  repulsion  between  the  states  at  any  time  instance. 
In  order  to  implement  this  interconnection  we  introduce 
F via  vl  for  each  subsystem  so  that  vl  now  becomes 
vl  =  —  xl  +  KlEl  +  El  and  equation  (9)  is  updated  to 

El  =KiEi  +  Flr 

To  study  the  stability  of  the  error  dynamics  for  the  ith 
subsystem  defined  in  the  above  equation  we  define  the 
Lyapunov  function  V \  similar  to  one  defined  in  equation 
(10)  and  calculate  its  derivative 

TP  =[KiEi  +  BFlr]TPiEi  +  EiTPi[KiEi  +  Flr\ 
=EiT[KiTPi  +  PiKi]Ei  +  2EiTPiFlr 
=  -  EiT Q^-E1  +  2EiTPiFlr 
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(13) 


where  pl  =  represents  the  x,y  and  z  position 

of  the  Ith  UAV  and  sl  =  [vl,vlz]T  represents  velocity  vector 
where  vl  is  the  linear  velocity  in  x  —  y  direction  and  v\  is  the 
velocity  in  2  direction.  6l  represents  the  planar  orientation  of 
the  UAV  and  uol  represents  the  angular  velocity  of  the  UAV. 
The  control  vector  is  u  =  [ui,  ^2,  ^3]T  with  us  =  ^F^—  g. 
Equation  (13)  resembles  equation  (1)  where  f(x'1)  and  g(xl) 
can  be  inferred.  Let  the  sensor  of  the  UAV  be  pointed  at  a 
fixed  angle  0,  0  <  </>  <  7 r,  with  the  horizontal  of  the  plane 
as  shown  in  Figure  1 .  Let  Li  be  the  horizontal  distance  from 
the  sensor  footprint  to  the  vertical  line  joining  the  sensor 
and  the  ground  as  shown  in  Figure  1.  It  is  clear  that  Li  is 
a  function  of  altitude,  z  =  Ez,  and  is  given  by 
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Error  signal:  We  define  the  error  as  the  sum  of  the 
difference  between  the  footprint  position  and  the  target 
position,  and  its  derivatives  so  that  error  dynamics  defined 
in  equation  (7)  is  nothing  but 

£*=/cM(r-Tr)  +  (r-rT) 

so  that  differentiating  it  with  respect  to  time  yields 

El=khi{r)  +  r 

The  error  dynamics  for  the  ith  UAV,  assuming  that  all  the 
kij  =  1,  is  given  by 


Fig.  1.  Figure  showing  the  UAV  and  sensor  arrangement 


Since  the  sensor  angle  is  fixed,  let  c  =  tan  </>,  a  constant.  Let 
the  output  of  the  system  (13),  be  given  by 


Ti 


x\  +  Li  cos(x3) 
x\  +  Li  sm(x3) 
x6 


(14) 


El  = 


x\  cos(x3)  —  Li  sin(4)4  +  cx7  cos(4) 
x\  sin(4)  +  Li  cos  (4)4  +  cx7  sin^g) 
x7 


xi 


+ 


—x\x\  sm(x3)  —  Li(xl)2  cos(x3)  —  2cx\x\  sin(x3) 
x\x\  cos(x3)  —  Li(xl)2  sin(4)  +  2cx\x\  cos(x3) 

0 


Ai{xi) 


where  the  first  two  rows  represent  the  x  and  y  position  of 
the  sensor  footprint  respectively  and  the  third  row  represents 
the  altitude  of  the  vehicle.  By  defining  the  output  as  given 
in  equation  (14)  we  are  not  only  controlling  the  footprint 
position  of  the  vehicle  but  also  controlling  the  altitude  of 
the  vehicle.  Differentiating  equation  (14)  with  respect  to  time 
gives 


x\  cos(x3)  —  Li  sm(x3)xl  +  cx7  cos(x3) 
x\  sin(xg)  +  Li  cos(x3)xl  +  cx7  sin(4) 
x7 


Differentiating  it  again  with  respect  to  time  yields 
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—x\x\  sm(x3)  —  Li{x l)2  cos(x3)  —  Clcx\x7  sin(4) 
x\x\  cos(x3)  —  Li(x\)2  sin(xg)  +  2cx\x\  cos  (4) 
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In  order  to  regulate  the  error  we  define  the  feedback  lin¬ 
earizing  controller  ul  to  be  the  one  given  in  equation  (5) 
with  vl  =  —xl  +  KlEl,  where  x1  is  defined  in  the  above 
equation  and  a  choice  of  stable  Kl  is  made.  The  choice  of 
such  a  controller  results  in  all  the  UAV’s  converging  to  the 
target  location  as  shown  in  Figure  2.  But  it  is  evident  from 
Figure  2  that  the  UAVs  collide  (blue  and  green  UAV)  with 
each  other  during  the  process  of  convergence. 


^  cos(4)  -  sin  x\  c  cos(xg) 
^  sin(4)  cos  c  sin(4) 


Since 


det 


b.  cos(4) 

i  sin(4) 
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u3  CC0S(4) 

Cg  csin(xg) 
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7^0 


the  system  (13)  with  output  (14)  has  a  vector  relative  degree 
of  [2,  2,  2]t  and  is  input-output  feedback  linearizable. 

B.  Presence  of  a  Single  Target 


Target  Model:  Here  we  consider  a  stationary  target  whose 
position  TT  =  [xT,yT,  zr\T  is  known  a  priori  either  by  a 

global  positioning  satellite  or  from  the  information  given  by  Flg'  2-  UAV  converge  towards  the  target  and  collide  with  each  other 
other  UAV. 
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C.  Force  of  Repulsion 

We  define  a  repulsive  profile  around  each  vehicle  similar 
to  the  one  define  in  equation  (12)  as 

N 

Flr  —  ^  Kj  exp 

With  this  definition  of  repulsive  force  it  is  clear  that  the 
repulsive  force  is  a  function  of  all  x,y  and  z  positions 
where  the  repulsion  increases  as  the  positions  of  the  vehicles 
approach  each  other,  affecting  in  turn  the  location  of  the 
footprint.  The  significance  of  is  that  even  if  the  output  of 
the  ith  and  jth  vehicle  become  equal  there  exists  a  certain 
minimum  force  of  repulsion  which  avoids  the  vehicles  from 
collision.  The  output  of  the  vehicles  become  equal  when  the 
angle  of  sensor  footprint,  </>,  for  each  vehicle  is  different.  But 
this  choice  of  repulsive  function  may  not  seem  to  be  practical 
all  the  time.  As  an  example  consider  the  positions  of  the  UAV 
footprints  as  shown  in  Figure  3  when  they  have  a  repulsive 
function  in  terms  of  x,y  and  2  position.  In  this  scenario  the 
target  is  placed  at  the  origin  and  the  vehicles  are  initially 
placed  at  (-100,  -100,  20),  (100,  100,  20)  and  (0,  100,  20) 
initially.  The  UAVs  converge  towards  the  target  with  their 
sensor  footprint  (the  rectangular  areas  in  the  figure)  placed 
in  a  invariant  set  surrounding  the  target  but  not  exactly  on  the 
target.  The  invariant  set,  though  very  small  in  this  example, 


UP  “P  H 


(r  -  rj  +  v) 


force  of  repulsion  between  the  vehicles  can  be  expressed 
mathematically  as 


FI  = 


'  0  if |  \pl  —  pj  1 1  >  S 
k  -( 
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3=1  J  A* 


~rJz\\Z 


(r-r^  +  ^)if  ll^-^ll  <s 


With  the  introduction  of  this  repulsive  force  it  is  evident  that 
the  UAV  converge  towards  a  smaller  invariant  set  around  the 
target  with  out  colliding  with  each  other  as  shown  in  Figure 
(4). 


Fig.  3.  UAV  converge  towards  target  with  repulsion  among  themselves 

may  increase  if  there  are  more  UAVs  that  converge  towards 
the  target.  The  question  now  arises  whether  the  invariant  set 
can  be  decreased  considerably  if  the  repulsion  profile  were  to 
be  a  function  of  altitude,  z,  alone.  But  if  the  repulsive  profile 
were  to  a  function  of  z  alone  the  UAV  could  face  a  force  of 
repulsion  when  they  are  at  same  altitude  but  located  far  away 
in  the  x—y  plane  which  is  not  necessary.  In  order  to  deal  with 
such  situation  we  define  a  repulsive  force  similar  to  equation 
(12)  but  as  function  of  altitude  z  and  the  distance  between 
the  vehicles.  We  define  a  circle  of  safe  distance  S  around 
the  UAVs,  inside  which  the  UAVs  experience  a  repulsive 
force  that  is  a  function  of  z  alone  and  outside  which  the 
UAVs  do  not  experience  any  force  of  repulsion.  Thus  the 


Fig.  4.  UAV  converge  towards  target  and  have  a  repulsive  profile  as  a 
function  of  altitude 

D.  Presence  of  Multiple  Targets 

Consider  the  case  when  we  have  more  than  one  target  and 
to  be  more  specific  the  case  when  we  have  more  UAVs  than 
targets.  In  such  instances  the  task  assignments  to  each  UAV 
plays  a  vital  role.  In  this  section  we  discuss  the  problem 
of  task  assignment  for  different  UAVs  and  the  scenario  for 
dealing  with  multiple  target  case. 

Define  a  closed  set  D  with  its  center  placed  at  the  center  of 
mass  of  multiple  targets.  In  order  to  simplify  our  assumption 
we  assume  that  D  is  rectangular  invariant  set  with  length  l 
and  width  w.  In  practice  this  set  D  can  be  considered  as  the 
search  area  in  which  the  UAVs  are  interested  in  looking  for 
target  and  assume  that  all  the  UAVs  are  initialized  outside 
this  search  area.  The  selected  invariant  set  D  is  such  that  D 
is  a  superset  of  all  the  invariant  sets  fT  for  i  =  1,  2, ...  M 
where  M  is  total  number  of  targets. 

E.  Task  Assignment  via  change  of  reference  signal 

Assume  that  there  exists  a  virtual  target  whose  position 
is  defined  as  the  center  of  mass  of  all  the  target  positions. 
The  virtual  target  position  is  assigned  as  the  goal  for  all  the 
UAVs  which  are  outside  the  set  D.  This  makes  the  UAVs 
be  attracted  towards  the  center  of  the  search  area  when 
they  are  outside  D.  Once  the  UAV  enters  the  set  D ,  task 
assignment  is  done  based  on  greedy  distance  criteria.  The 
task  assignment  algorithm  at  present  works  as  follows.  Each 
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UAV  calculates  its  distance  to  all  the  target  positions  and 
communicates  it  to  all  the  other  UAVs.  Therefore  at  a  given 
time  instance  all  the  UAVs  know  the  distance  between  the 
UAVs  and  the  targets  present  in  the  search  area.  Based  on  this 
information  each  UAV  is  assigned  the  closest  target  (which  is 
a  reference  signal  in  case  of  general  MIMO  systems).  In  the 
task  assignment  process  all  the  targets  are  assigned  to  UAVs 
and  this  assignment  changes  dynamically.  UAVs  to  which 
targets  are  not  assigned  will  converge  towards  the  center  of 
mass  of  the  target.  Thus  center  of  mass  of  the  targets  acts  like 
a  default  target  for  all  the  UAVs.  Figure  5  shows  the  scheme 
of  how  the  task  assignment  works.  In  the  figure  there  are 
two  targets  placed  at  (40,  0,  0)  and  (-40,  0,  0)  respectively. 
Two  of  the  UAVs  (red  and  blue)  are  assigned  to  two  different 
targets  and  the  third  UAV  (green)  is  assigned  the  center  of 
mass  of  the  targets. 
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Fig.  5.  Task  Assignment 


IV.  Conclusion  and  Future  Research 

In  this  paper  we  have  considered  a  control  scheme  for 
the  three  dimensional  cooperative  path  planning  of  inter¬ 
connected  systems  in  general  and  have  shown  how  this 
theory  can  be  applicable  for  path  planning  of  autonomous  air 
vehicles  to  reach  their  dynamically  assigned  goal  positions. 
Lyapunov  stability  analysis  is  used  not  only  to  show  the 
stability  of  the  system  but  also  the  conditions  under  which 
such  systems  are  stable.  Future  work  involves  the  stability 
of  such  systems  under  dynamical  constraints  placed  on  some 
states  (example  saturating  the  velocity  in  case  of  UAVs) 
and  extending  it  to  dynamical  environment  which  involves 
moving  targets  and  obstacles. 
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